#include "pausestate.h"
#include "fsm.h"
#include "maincontroller.h"
#include "masters/vehiclecontroller.h"
#include "device/dhdc.h"
#include "device/drdc.h"
using namespace std;
using namespace vehicle;
PauseState::PauseState()
{

}

/**
 * @brief PauseState::Update
 * @param fsm

 */
void PauseState::Update(FSM *fsm)
{
    switch (fsm->vehicle_controller_->GetRunningStatus())
    {
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
    {
        char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();

        ushort sn=0;
        int ret=dhdGetSerialNumber(&sn,master_device);
        if(ret!=0)
        {
            fsm->vehicle_controller_->SetMasterDeviceIndex(-1);
            master_device=-1;
        }

        if(master_device>=0)
        {
            if(fsm->vehicle_controller_->GetConnectionStatus()==main_controller::kConnectionOkay)
            {
                fsm->SetState(BaseState::kResetState);
            }
        }
    }
        break;
    default:
        break;
    }

}

void PauseState::OnEntry(FSM *fsm)
{
    cout<<"entry pause state"<<endl;
    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();
    if(master_device<0)
        return;
    dhdSetForceAndGripperForce(0,0,0,0,master_device);

    drdRegulatePos(true,master_device);
    drdHold(master_device);

//    fsm->vehicle_controller_->GetMasterEndPosition(fsm->vehicle_controller_->master_target_pos_);
    //TODO SET SLAVE TARGET
    //fsm->vehicle_controller_->SetSlaveTargetPose(M2S*(master_stable_pos_-fsm->vehicle_controller_->master_origin_pos_)+fsm->vehicle_controller_->slave_origin_pos_);
}

void PauseState::OnExit(FSM *fsm)
{
    cout<<"exit pause state"<<endl;
}



